#include "XMath.h"

inline float XSqrt(float F)
{
	const float fZero = 0.0f;
	const float fThree = 3.0f;
	const float fOneHalf = 0.5f;
	float temp;

	__asm
	{
		movss	xmm1,[F]
		rsqrtss xmm0,xmm1			// 1/sqrt estimate (12 bits)

		// Newton-Raphson iteration (X1 = 0.5*X0*(3-(Y*X0)*X0))
		movss	xmm3,[fThree]
		movss	xmm2,xmm0
		mulss	xmm0,xmm1			// Y*X0
		mulss	xmm0,xmm2			// Y*X0*X0
		mulss	xmm2,[fOneHalf]		// 0.5*X0
		subss	xmm3,xmm0			// 3-Y*X0*X0
		mulss	xmm3,xmm2			// 0.5*X0*(3-Y*X0*X0)

		movss	xmm4,[fZero]
		cmpss	xmm4,xmm1,4			// not equal

		mulss	xmm3,xmm1			// sqrt(f) = f * 1/sqrt(f)

		andps	xmm3,xmm4			// seet result to zero if input is zero

		movss	[temp],xmm3
	}

	return temp;
}

__declspec(naked) void MatrixMult(float *src1, float *src2, float *dst) 
{
	__asm {
		mov edx, dword ptr [esp+4] ; src1
			mov eax, dword ptr [esp+0Ch] ; dst
			mov ecx, dword ptr [esp+8] ; src2
			movss xmm0, dword ptr [edx]
			movups xmm1, xmmword ptr [ecx]
			shufps xmm0, xmm0, 0
			movss xmm2, dword ptr [edx+4]
			mulps xmm0, xmm1
			shufps xmm2, xmm2, 0
			movups xmm3, xmmword ptr [ecx+10h]
			movss xmm7, dword ptr [edx+8]
			mulps xmm2, xmm3
			shufps xmm7, xmm7, 0
			addps xmm0, xmm2
			movups xmm4, xmmword ptr [ecx+20h]
			movss xmm2, dword ptr [edx+0Ch]
			mulps xmm7, xmm4
			shufps xmm2, xmm2, 0
			addps xmm0, xmm7
			movups xmm5, xmmword ptr [ecx+30h]
			movss xmm6, dword ptr [edx+10h]
			mulps xmm2, xmm5
			movss xmm7, dword ptr [edx+14h]
			shufps xmm6, xmm6, 0
			addps xmm0, xmm2
			shufps xmm7, xmm7, 0
			movlps qword ptr [eax], xmm0
			movhps qword ptr [eax+8], xmm0
			mulps xmm7, xmm3
			movss xmm0, dword ptr [edx+18h]
			mulps xmm6, xmm1
			shufps xmm0, xmm0, 0
			addps xmm6, xmm7
			mulps xmm0, xmm4
			movss xmm2, dword ptr [edx+24h]
			addps xmm6, xmm0
			movss xmm0, dword ptr [edx+1Ch]
			movss xmm7, dword ptr [edx+20h]
			shufps xmm0, xmm0, 0
			shufps xmm7, xmm7, 0
			mulps xmm0, xmm5
			mulps xmm7, xmm1
			addps xmm6, xmm0
			shufps xmm2, xmm2, 0
			movlps qword ptr [eax+10h], xmm6
			movhps qword ptr [eax+18h], xmm6
			mulps xmm2, xmm3
			movss xmm6, dword ptr [edx+28h]
			addps xmm7, xmm2
			shufps xmm6, xmm6, 0
			movss xmm2, dword ptr [edx+2Ch]
			mulps xmm6, xmm4
			shufps xmm2, xmm2, 0
			addps xmm7, xmm6
			mulps xmm2, xmm5
			movss xmm0, dword ptr [edx+34h]
			addps xmm7, xmm2
			shufps xmm0, xmm0, 0
			movlps qword ptr [eax+20h], xmm7
			movss xmm2, dword ptr [edx+30h]
			movhps qword ptr [eax+28h], xmm7
			mulps xmm0, xmm3
			shufps xmm2, xmm2, 0
			movss xmm6, dword ptr [edx+38h]
			mulps xmm2, xmm1
			shufps xmm6, xmm6, 0
			addps xmm2, xmm0
			mulps xmm6, xmm4
			movss xmm7, dword ptr [edx+3Ch]
			shufps xmm7, xmm7, 0
			addps xmm2, xmm6
			mulps xmm7, xmm5
			addps xmm2, xmm7
			movups xmmword ptr [eax+30h], xmm2
			ret
	} // asm
}

